/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 02:16:25
 * @Description: ...
 * @Reference: ...
 */

#ifndef SRC_VELODYNE_CONVERTER_H
#define SRC_VELODYNE_CONVERTER_H
#include <algorithm>
#include <string>
#include "converter.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

struct VelodynePointXYZIRT {
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(
    VelodynePointXYZIRT,
    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
                                            intensity)(uint16_t, ring,
                                                       ring)(float, time, time))

namespace kaist_publisher {

class VelodyneConverter : public Converter {
public:
    typedef std::shared_ptr<VelodyneConverter> Ptr;
    VelodyneConverter(ros::NodeHandle& nh, const std::string& dataset_dir,
                      const std::string& save_dir,
                      const std::string& left_topic,
                      const std::string& right_topic);
    virtual ~VelodyneConverter() = default;

    int Convert() override;

    std::string default_left_stamp_file = "sensor_data/VLP_left_stamp.csv";
    std::string default_left_data_dir = "sensor_data/VLP_left";
    std::string default_right_stamp_file = "sensor_data/VLP_right_stamp.csv";
    std::string default_right_data_dir = "sensor_data/VLP_right";

    int64_t Init_Publish();
    int64_t Publish();
    void Publish(int64_t& timestamp, sensor_msgs::PointCloud2::Ptr& left_lidar,
                 sensor_msgs::PointCloud2::Ptr& right_lidar);

    ros::Publisher pub_velodyne_left_;
    ros::Publisher pub_velodyne_right_;
    std::vector<std::pair<int64_t, std::string>> all_stamps_;
    int idx_ = 0;

private:
    std::string left_topic_;
    std::string left_bag_name_;
    std::string right_topic_;
    std::string right_bag_name_;

    void Convert(const std::string& stamp_file, const std::string& data_dir,
                 const std::string& bag_file, const std::string& topic,
                 const std::string& frame_id);
};

}  // namespace kaist_publisher
#endif  // SRC_VELODYNE_CONVERTER_H
